//
// Created by lee on 24-3-20.
//

#ifndef DKS_AGX_STRUCT_AGX_RECEIVE_H
#define DKS_AGX_STRUCT_AGX_RECEIVE_H

#include <cstdint>
#include <cstring>
#include <unistd.h>
#include <iostream>

#pragma pack(push,1)
struct px4_to_agx_packet_t{
    uint8_t     node_id;           // 节点id 0x00-0xa0
    uint8_t     gps_time;
    uint32_t    nav_pac_cnt;          //单位？ 导航包计数
    uint8_t     nav_status;         // 导航状态 0x11卫星惯导 0x22惯导  0x33错误
    int32_t     lon;               // 经纬度，1e-7deg
    int32_t     lat;
    int32_t     alt;               // 高度 1e-3
    int32_t     roll_rate;         // 角速度单位1e-4deg/s 弹体坐标系
    int32_t     pitch_rate;
    int32_t     yaw_rate;
    int32_t     roll;              // 角度单位1e-4deg  弹体坐标与地理坐标夹角
    int32_t     pitch;
    int32_t     yaw;
    int16_t     v_east;            // cm/s，1e-2m/s
    int16_t     v_north;
    int16_t     v_up;
    int16_t     velocity;           //合速度
    int32_t     acc_x;             // 1e-4m/s2 不含地球引力在内的绝对加速度在弹体坐标下的投影
    int32_t     acc_y;
    int32_t     acc_z;
    // DKS新增，不知道什么意思
    uint8_t NavFlag;
    uint8_t PodStatus1;
	uint8_t PodStatus2;
    uint8_t PodStatus3;
	uint8_t PodStatus4;
    int16_t PodxShift;
    int16_t PodyShift;
    int16_t PodRoll;
    int16_t PodPitch;
    int16_t PodYaw;
    int16_t Podatt_Roll;
    int16_t Podatt_Pitch;
    int16_t PodrollAngleVelocity;
    int16_t PodpitchAngleVelocity;
    int16_t PodyawAngleVelocity;
    uint16_t PodLasDistance;
    uint8_t PodChekFlag;
};
#pragma pack(pop)

constexpr int send_to_agx_size = sizeof (px4_to_agx_packet_t) + 4 + 2;
const uint8_t agx_to_px4_payload_size = sizeof(px4_to_agx_packet_t);

void send_packet_to_agx(int uartFd, px4_to_agx_packet_t &packet) {
    uint8_t sendbuff[send_to_agx_size]{};
    uint8_t packet_head[4]={0x55, 0xaa, agx_to_px4_payload_size, 0x07}; //帧头1、帧头2、有效数据长度、表示该数据是导航数据
    memcpy(&sendbuff[0], packet_head, 4);
    memcpy(&sendbuff[4], &packet, agx_to_px4_payload_size);

    uint16_t check_sum = get_checksum(&sendbuff[2], agx_to_px4_payload_size+2); //索引2到包尾，头尾都包含
    sendbuff[send_to_agx_size -2] = check_sum & 0x00ff;
    sendbuff[send_to_agx_size -1] = check_sum >> 8;

    ssize_t send_length = write(uartFd, sendbuff, send_to_agx_size);
    std::cout << "send package to px4, length " << (int)send_length << std::endl;
}



#endif //DKS_AGX_STRUCT_AGX_RECEIVE_H
